#include "action_01/robot.h"
#include "action_interface/action/move_robot.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

class Robot01 : public rclcpp::Node {
public:
  using MoveRobot = action_interface::action::MoveRobot;
  using GoalHandleMoveRobot = rclcpp_action::ServerGoalHandle<MoveRobot>;
  Robot01(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "Node: %s...", name.c_str());
    using namespace std::placeholders;
    action_server_ = rclcpp_action::create_server<MoveRobot>(
        this, "move_robot", std::bind(&Robot01::_handle_goal, this, _1, _2),
        std::bind(&Robot01::_handle_cancel, this, _1),
        std::bind(&Robot01::_handle_accepted, this, _1));
  }

private:
  rclcpp_action::GoalResponse
  _handle_goal(const rclcpp_action::GoalUUID &uuid,
               std::shared_ptr<const MoveRobot::Goal> goal) {
    RCLCPP_INFO(this->get_logger(), "Recieved goal request with distance %f",
                goal->distance);
    (void)uuid;
    if (fabs(goal->distance > 100)) {
      // if(goal->distance>100){
      RCLCPP_WARN(this->get_logger(), "Goal too far,rejected...");
      return rclcpp_action::GoalResponse::REJECT;
    }
    RCLCPP_INFO(this->get_logger(), "Distance %f is reachable,ready go...",
                goal->distance);
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  rclcpp_action::CancelResponse
  _handle_cancel(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "Recieved request to cancel goal...");
    (void)goal_handle;
    //如果没有操作，就默认只需要三行代码
    robot_.stop_move();
    return rclcpp_action::CancelResponse::ACCEPT;
  }
  void _exeecute_move(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    const auto goal = goal_handle->get_goal();
    // goal_handle->get_goal_id();
    RCLCPP_INFO(this->get_logger(), "Start to move to distance %f...",
                goal->distance);
    auto result = std::make_shared<MoveRobot::Result>();
    rclcpp::Rate rate = rclcpp::Rate(2);
    robot_.set_goal(goal->distance);
    while (rclcpp::ok() && !robot_.close_goal()) {
      robot_.move_step();
      auto feedback = std::make_shared<MoveRobot::Feedback>();
      feedback->pose = robot_.get_current_pose();
      feedback->status = robot_.get_status();
      goal_handle->publish_feedback(feedback);
      if (goal_handle->is_canceling()) {
        result->pose = robot_.get_current_pose();
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal canceled...");
        return;
      }
    }
    RCLCPP_INFO(this->get_logger(), "Pulished feedback...");
    rate.sleep();
  }
  void
  _handle_accepted(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    using namespace std::placeholders;
    std::thread{std::bind(&Robot01::_exeecute_move, this, _1), goal_handle}
        .detach();
  }

private:
  Robot robot_{};
  rclcpp_action::Server<MoveRobot>::SharedPtr action_server_{};
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<Robot01>("robot_01");
  rclcpp::spin(node);
  rclcpp::shutdown();

  return 0;
}